#include "WholeBodyDynamicSensorVisualisation.h"

#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoMaterial.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>

#include <tuple>

using namespace MMM;

WholeBodyDynamicSensorVisualisation::WholeBodyDynamicSensorVisualisation(WholeBodyDynamicSensorPtr sensor, SoSeparator* sceneSep) :
    SensorVisualisation(sceneSep),
    sensor(sensor)
{
}

std::string WholeBodyDynamicSensorVisualisation::getType() {
    return sensor->getType();
}

int WholeBodyDynamicSensorVisualisation::getPriority() {
    return sensor->getPriority();
}

Eigen::Matrix4f WholeBodyDynamicSensorVisualisation::createCOMPose(const Eigen::Vector3f &centerOfMass) {
    Eigen::Vector3f com = centerOfMass;
    Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
    //com(2) = 0.0f;
    globalPose.block(0, 3, 3, 1) = com;
    return globalPose;
}

void WholeBodyDynamicSensorVisualisation::setPose(const Eigen::Vector3f &pose, const Eigen::Vector3f &direction) {
    SoMatrixTransform* mt = (SoMatrixTransform*) getChildVisualisationSep(1);
    SbMatrix m_(reinterpret_cast<SbMat*>(WholeBodyDynamicSensorVisualisation::createCOMPose(pose).data()));
    mt->matrix.setValue(m_);
    removeChildVisualisationSep(3);
    addChildVisualisationSep(VirtualRobot::CoinVisualizationFactory::CreateArrow(direction, calculateLength(direction), 10.0f, VirtualRobot::CoinVisualizationFactory::Color::Red()));
}

float WholeBodyDynamicSensorVisualisation::calculateLength(const Eigen::Vector3f &vector) {
    return std::sqrt(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2));
}

void WholeBodyDynamicSensorVisualisation::update(float timestep, float delta) {
    update(sensor->getDerivedMeasurement(timestep, delta));
}

void WholeBodyDynamicSensorVisualisation::update(float timestep) {
    update(sensor->getDerivedMeasurement(timestep));
}

void WholeBodyDynamicSensorVisualisation::update(WholeBodyDynamicSensorMeasurementPtr measurement) {
    if (!measurement) return;

    if (!hasVisualisationSep()) {
        SoSeparator* comSep = new SoSeparator;
        SoMaterial* marked = new SoMaterial;
        marked->ambientColor.setValue(1.0f, 0.2f, 0.2f);
        marked->diffuseColor.setValue(1.0f, 0.2f, 0.2f);
        marked->specularColor.setValue(0.5f, 0.5f, 0.5f);
        comSep->addChild(marked);
        Eigen::Matrix4f centerOfMass = WholeBodyDynamicSensorVisualisation::createCOMPose(measurement->getCenterOfMass());
        comSep->addChild(VirtualRobot::CoinVisualizationFactory::getMatrixTransform(centerOfMass));
        SoSphere* sphere = new SoSphere();
        sphere->radius = 50;
        comSep->addChild(sphere);
        comSep->addChild(VirtualRobot::CoinVisualizationFactory::CreateArrow(measurement->getAngularMomentum(), calculateLength(measurement->getAngularMomentum()), 10.0f, VirtualRobot::CoinVisualizationFactory::Color::Red()));
        setVisualisationSep(comSep);
    } else {
        setPose(measurement->getCenterOfMass(), measurement->getAngularMomentum());
    }
}
